/*
 * ConnectPoints.cpp
 *
 *  Created on: 2010-02-25
 *      Author: revelboot
 */

#include "ConnectPoints.h"

namespace common {

ConnectPoints::ConnectPoints(int id,Point* k,Point* l): common::Constraint(id,2,CONNECT_POINTS){
	setK(k);
	setL(l);
}
ublasVector ConnectPoints::getValue() throw(MyException){
	// K-L
	ublasVector v(2);
	v(0) = k->getX() - l->getX();
	v(1) = k->getY() - l->getY();
	return v;
}
ublasMatrix ConnectPoints::getJacobianOnPoint(Point* p) throw(MyException){


	if(k==p){
		return ublasMatrixIdentity(2,2);
	}
	else if(l==p){
		return ublasMatrixIdentity(2,2)*(-1);
	}
	return ublasMatrixZero(0,0);
}
ConnectPoints::~ConnectPoints() {

}

}
